﻿using System;
using System.Collections.Generic;
using System.Text;
using xn;
using xnv;
using System.Threading;
using Mogre;

namespace AplikacjaOgre
{
    class KinektController
    {
        Context context;

        UserGenerator userGenerator;
        HandsGenerator handsGenerator;
        GestureGenerator gestureGenerator;
        DepthGenerator depthGenerator;

        public SceneMetaData currentSceneMetaData;

        SessionManager sessionManager;

        public uint currentUserId = 0;

        SkeletonCapability skeletonCapability;

        Player player;

        public bool active = true;

        public Thread kamera;

        public KinektController()
        { }

        public void BeginControlling(Player player)
        {
            this.player = player;
            context = new Context("config.xml");

            checkGenerators();

            sessionManager = new SessionManager(context, "Click", "RaiseHand");
            sessionManager.SetQuickRefocusTimeout(0);

            //handsGenerator.SetSmoothing(1);

            userGenerator.NewUser += new UserGenerator.NewUserHandler(userGenerator_NewUser);
            userGenerator.LostUser += new UserGenerator.LostUserHandler(userGenerator_LostUser);
            //depthGenerator.GetMirrorCap().SetMirror(true);

            skeletonCapability = userGenerator.GetSkeletonCap();
            skeletonCapability.SetSkeletonProfile(SkeletonProfile.All);
            skeletonCapability.SetSmoothing(0.6f);
            skeletonCapability.CalibrationEnd += new SkeletonCapability.CalibrationEndHandler(skeletonCapability_CalibrationEnd);

            context.StartGeneratingAll();

            kamera = new Thread(KameraThread);
            kamera.IsBackground = true;
            kamera.Start();
        }

        private void skeletonCapability_CalibrationEnd(ProductionNode productionNode, uint userId, bool succes)
        {
            if (succes)
            {
                skeletonCapability.StartTracking(userId);

                System.Console.WriteLine("Kalibracja skończona");

                //SkeletonJointPosition torso = new SkeletonJointPosition();
                //skeletonCapability.GetSkeletonJointPosition(userId, SkeletonJoint.Torso, ref torso);
            }
        }

        private void userGenerator_PoseEnded(ProductionNode productionNode, String pose, uint userId)
        {
            System.Console.WriteLine("Poza stracona");
        }

        private void userGenerator_NewUser(ProductionNode productionNode, uint userId)
        {
            if (currentUserId == 0)
            {
                currentUserId = userId;
                userGenerator.GetSkeletonCap().RequestCalibration(userId, false);

                System.Console.WriteLine("Pojawił się ktoś do sterowania");
            }
        }

        private void userGenerator_LostUser(ProductionNode productionNode, uint userId)
        {
            if (currentUserId == userId)
            {
                currentUserId = 0;
            }

            System.Console.WriteLine("User uciekł");
        }

        private void checkGenerators()
        {
            userGenerator = context.FindExistingNode(NodeType.User) as UserGenerator;
            if (userGenerator == null)
            {
                throw new Exception("Brak User Node");
            }

            handsGenerator = context.FindExistingNode(NodeType.Hands) as HandsGenerator;
            if (handsGenerator == null)
            {
                throw new Exception("Brak Hand Node");
            }

            gestureGenerator = context.FindExistingNode(NodeType.Gesture) as GestureGenerator;
            if (gestureGenerator == null)
            {
                throw new Exception("Brak Gesture Node");
            }

            depthGenerator = context.FindExistingNode(NodeType.Depth) as DepthGenerator;
            if (depthGenerator == null)
            {
                throw new Exception("Brak Depth Node");
            }
        }

        private void KameraThread()
        {
            while (active)
            {
                DepthMetaData depthMD = new DepthMetaData();
                
                context.WaitOneUpdateAll(depthGenerator);
                sessionManager.Update(context);

                depthGenerator.GetMetaData(depthMD);

                //SkeletonJointOrientation j = new SkeletonJointOrientation();
                //userGenerator.GetSkeletonCap().GetSkeletonJointOrientation(currentUserId, SkeletonJoint.LeftShoulder, j);



                if (currentUserId != 0)
                {
                    updateDepthImage();

                    if(skeletonCapability.IsCalibrated(currentUserId))
                        controlPlayer();
                }
            }

            sessionManager.EndSession();

        }

        unsafe void updateDepthImage()
        {
            currentSceneMetaData = userGenerator.GetUserPixels(0);
        }

        void moveBone(String boneName, SkeletonJoint skeletonJoint)
        {
            Bone bone = player.Entity.Skeleton.GetBone(boneName);
            Quaternion initialOrientation = bone.InitialOrientation;
            Quaternion newOrientation = Quaternion.IDENTITY;

            SkeletonJointOrientation joint = new SkeletonJointOrientation();
            userGenerator.GetSkeletonCap().GetSkeletonJointOrientation(currentUserId, skeletonJoint, joint);

            if (joint.Confidence > 0)
            {
                Matrix3 rotationMatrix = new Matrix3(joint.Orientation.elements[0], -joint.Orientation.elements[1], joint.Orientation.elements[2],
                                                        -joint.Orientation.elements[3], joint.Orientation.elements[4], -joint.Orientation.elements[5],
                                                        joint.Orientation.elements[6], -joint.Orientation.elements[7], joint.Orientation.elements[8]);

                newOrientation.FromRotationMatrix(rotationMatrix);

                bone.ResetOrientation();
                newOrientation = bone.ConvertWorldToLocalOrientation(newOrientation);
                newOrientation = newOrientation * initialOrientation;

                bone.SetOrientation(newOrientation.w, newOrientation.x, newOrientation.y, newOrientation.z);
            }
        }

        void controlPlayer()
        {
            moveBone("Humerus.L", SkeletonJoint.LeftShoulder);
            moveBone("Humerus.R", SkeletonJoint.RightShoulder);
            moveBone("Ulna.L", SkeletonJoint.LeftElbow);
            moveBone("Ulna.R", SkeletonJoint.RightElbow);
            moveBone("Chest", SkeletonJoint.Torso);
            moveBone("Stomach", SkeletonJoint.Torso);
            moveBone("Thigh.L", SkeletonJoint.LeftHip);
            moveBone("Thigh.R", SkeletonJoint.RightHip);
            moveBone("Calf.L", SkeletonJoint.LeftKnee);
            moveBone("Calf.R", SkeletonJoint.RightKnee);
        }

    }
}
